use futures::stream::StreamExt;
use r2r::{self, QosProfile};
use r2r::{chapt4_interfaces, geometry_msgs, rcl_interfaces, turtlesim};
use std::sync::{Arc, Mutex};
use tokio;

#[tokio::main]
async fn main() -> std::result::Result<(), Box<dyn std::error::Error>> {
    let ctx = r2r::Context::create()?;
    let node = r2r::Node::create(ctx, "turtle_controller", "")?;
    let arc_node = Arc::new(Mutex::new(node));

    let nl = arc_node.lock().unwrap().logger().to_string();

    let _k = match arc_node.lock().unwrap().get_parameter::<f64>("k") {
        Ok(val) => val,
        Err(_) => 1.0,
    };
    let _max_speed = match arc_node.lock().unwrap().get_parameter::<f64>("max_speed") {
        Ok(val) => val,
        Err(_) => 1.0,
    };

    let target_x_: Arc<Mutex<f32>> = Arc::new(Mutex::new(1.0));
    let target_y_: Arc<Mutex<f32>> = Arc::new(Mutex::new(1.0));
    let k_: Arc<Mutex<f32>> = Arc::new(Mutex::new(_k as f32));
    let max_speed_: Arc<Mutex<f32>> = Arc::new(Mutex::new(_max_speed as f32));

    let node_clone = arc_node.clone();
    let velocity_pub = node_clone
        .lock()
        .unwrap()
        .create_publisher::<geometry_msgs::msg::Twist>("/turtle1/cmd_vel", QosProfile::default())?;

    let node_clone = arc_node.clone();
    let mut patrol_srv = node_clone
        .lock()
        .unwrap()
        .create_service::<chapt4_interfaces::srv::Patrol::Service>(
            "/patrol",
            QosProfile::default(),
        )?;

    let target_x_clone = target_x_.clone();
    let target_y_clone = target_y_.clone();
    let nl_clone: String = nl.clone();
    tokio::spawn(async move {
        loop {
            match patrol_srv.next().await {
                Some(request) => {
                    let mut target_x = target_x_clone.lock().unwrap();
                    let mut target_y = target_y_clone.lock().unwrap();

                    r2r::log_info!(
                        nl_clone.as_str(),
                        "接收到目标点请求: ({}, {})",
                        request.message.target_x,
                        request.message.target_y
                    );

                    if (request.message.target_x > 0. && request.message.target_x < 12.0)
                        && (request.message.target_y > 0. && request.message.target_y < 12.0)
                    {
                        *target_x = request.message.target_x;
                        *target_y = request.message.target_y;

                        let mut response = chapt4_interfaces::srv::Patrol::Response::default();
                        response.result = 1;
                        request
                            .respond(response)
                            .expect("could not send service response");
                    } else {
                        let mut response = chapt4_interfaces::srv::Patrol::Response::default();
                        response.result = 0;
                        request
                            .respond(response)
                            .expect("could not send service response");
                    }
                }
                None => break,
            }
        }
    });

    let node_clone = arc_node.clone();
    let mut pose_sub = node_clone
        .lock()
        .unwrap()
        .subscribe::<turtlesim::msg::Pose>("/turtle1/pose", QosProfile::default())?;

    let target_x_clone = target_x_.clone();
    let target_y_clone = target_y_.clone();
    let k_clone = k_.clone();
    let max_speed_clone = max_speed_.clone();
    let nl_clone = nl.clone();
    tokio::spawn(async move {
        loop {
            let mut message = geometry_msgs::msg::Twist::default();

            match pose_sub.next().await {
                Some(pose) => {
                    let current_x = pose.x;
                    let current_y = pose.y;

                    r2r::log_info!(
                        nl_clone.as_str(),
                        "当前位置:(x={},y={})",
                        current_x,
                        current_y
                    );

                    let target_x = *target_x_clone.lock().unwrap();
                    let target_y = *target_y_clone.lock().unwrap();
                    let k = *k_clone.lock().unwrap();
                    let max_speed = *max_speed_clone.lock().unwrap();

                    let distance =
                        ((target_x - current_x).powi(2) + (target_y - current_y).powi(2)).sqrt();
                    let mut angle = (target_y - current_y).atan2(target_x - current_x) - pose.theta;

                    // Normalize angle to [-π, π]
                    while angle > std::f32::consts::PI {
                        angle -= 2.0 * std::f32::consts::PI;
                    }
                    while angle < -std::f32::consts::PI {
                        angle += 2.0 * std::f32::consts::PI;
                    }

                    // r2r::log_info!(nl_clone.as_str(),
                    //     "目标位置:({}, {}), 距离:{:.2}, 角度:{:.2}",
                    //     target_x, target_y, distance, angle
                    // );

                    if distance > 0.1 {
                        if angle.abs() > 0.2 {
                            message.angular.z = angle as f64;
                            r2r::log_info!(nl_clone.as_str(), "旋转: {}", message.angular.z);
                        } else {
                            message.linear.x = (k * distance).min(max_speed) as f64;

                            r2r::log_info!(nl_clone.as_str(), "前进: {}", message.linear.x);
                        }
                    } else {
                        r2r::log_info!(
                            nl_clone.as_str(),
                            "已到达目标点 ({}, {})",
                            target_x,
                            target_y
                        );
                        message.linear.x = 0.0;
                        message.angular.z = 0.0;
                    }

                    let _ = velocity_pub.publish(&message);
                    
                }
                None => {
                    r2r::log_error!(nl_clone.as_str(), "Failed to get post");
                }
            }
        }
    });

    let node_clone = arc_node.clone();
    let mut param_srv = node_clone
        .lock()
        .unwrap()
        .create_service::<r2r::rcl_interfaces::srv::SetParameters::Service>(
        "/turtle_controller/set_parameters",
        QosProfile::default(),
    )?;

    let k_clone = k_.clone();
    let max_speed_clone = max_speed_.clone();
    let nl_clone = nl.clone();
    tokio::spawn(async move {
        loop {
            match param_srv.next().await {
                Some(request) => {
                    let mut results = vec![];
                    for param in &request.message.parameters {
                        let mut success = true;
                        let mut reason = String::new();
                        match param.name.as_str() {
                            "k" => {
                                let val = param.value.double_value;
                                *k_clone.lock().unwrap() = val as f32;
                                r2r::log_info!(nl_clone.as_str(), "Received parameters: k={}", val);
                            }
                            "max_speed" => {
                                let val = param.value.double_value;
                                *max_speed_clone.lock().unwrap() = val as f32;
                                r2r::log_info!(
                                    nl_clone.as_str(),
                                    "Received parameters: max_speed={}",
                                    val
                                );
                            }
                            _ => {
                                success = false;
                                reason = "unknown parameter".to_string();
                            }
                        }
                        results.push(rcl_interfaces::msg::SetParametersResult {
                            successful: success,
                            reason,
                        });
                    }
                    request
                        .respond(rcl_interfaces::srv::SetParameters::Response { results })
                        .expect("could not send service response");
                }
                None => break,
            }
        }
    });

    let handle = tokio::task::spawn_blocking(move || loop {
        {
            arc_node
                .lock()
                .unwrap()
                .spin_once(std::time::Duration::from_millis(10));
        }
        // std::thread::sleep(std::time::Duration::from_millis(10))
    });

    handle.await?;

    Ok(())
}
